#include "bits/stdc++.h"
#include <ros/ros.h>
#include <std_msgs/Float32MultiArray.h>

#include "item.h"

using namespace std;

int main(int argc, char** argv)
{
    ros::init(argc, argv, "force");
    ros::NodeHandle nh;

    // params
    double K, gamma, n0, lambda, k_r_t, F;
    // nh.getParam("/K", K);
    // nh.getParam("/n0", n0);
    // nh.getParam("/gamma", gamma);
    // nh.getParam("/lambda", lambda);
    // nh.getParam("/k_r_t", k_r_t);
    // nh.getParam("/F", F);

    K = 1.0;
    gamma = 1.0;
    n0 = 1000.0;
    lambda = 1.0;
    k_r_t = 1.0;
    F = 1.0;

    Items items(nh, K, gamma, n0, lambda, k_r_t, F);

    ros::Subscriber items_sub = nh.subscribe<std_msgs::Float64MultiArray>("/pos_vel", 10, boost::bind(&Items::feed, &items, _1));

    // wait for connection
    ros::Rate rate(20.0);

    // wait for FCU connection
    while ( ros::ok() ) {



        static int cnt = 0;
        if ( cnt == 10 ) {


            cnt = 0;
        }
        cnt++;

        ros::spinOnce();
        rate.sleep();
    }


    return 0;
}
